/*********************************************************************************
  *Copyright(C),1996-2022,ChenJiehong
  *FileName:  pid.c
  *Author: ChenJiehong
  *Version:  V1.0
  *Date: 2023-04-24
***********************************************************************************/

/***************************************Includes***********************************/
#include "pid.h"


/***************************************Variables***********************************/
void filter_pid_Init (FILTER_PID_Object_t*  a_pHandler)
{
    a_pHandler->i16LastError = 0;
    a_pHandler->i32Integral = 0;
    a_pHandler->i32Error_Sum = 0;
}

int16_t filter_pid_Handler (FILTER_PID_Object_t*  a_pHandler, int16_t a_i16Error)
{
	int16_t i16RetVal;

    /* Derivative-part */
    int32_t i32PID_Dpart = 0;
    if (a_pHandler->u16KD != 0u)
    {
        i32PID_Dpart = (a_i16Error - a_pHandler->i16LastError)/a_pHandler->u16KD;

        a_pHandler->i16LastError = a_i16Error;
    }

    /* Integral-part */
    int32_t i32PID_Ipart;
    if(a_i16Error< 200 && a_i16Error > -200)
    {
        i32PID_Ipart = a_i16Error*a_pHandler->u16KI/100;
        a_pHandler->i32Error_Sum += i32PID_Ipart;
    }
    else
    {
        
    }

    if(a_pHandler->i32Error_Sum > a_pHandler->Integral_LimitMax)
    {
    	a_pHandler->i32Error_Sum = a_pHandler->Integral_LimitMax;
    }
    else if(a_pHandler->i32Error_Sum < (int32_t)a_pHandler->Integral_LimitMax*(-1))
    {
    	a_pHandler->i32Error_Sum = (int32_t)a_pHandler->Integral_LimitMax*(-1);
    }
    else
    {

    }
   
    a_pHandler->i32Integral = a_i16Error * a_pHandler->u16KP + a_pHandler->i32Error_Sum + i32PID_Dpart;
    a_pHandler->i32Integral = a_pHandler->i32Integral;

    if(a_pHandler->i32Integral > a_pHandler->u16OutputLimitMax)
    {
    	a_pHandler->i32Integral = a_pHandler->u16OutputLimitMax;
    }
    else if(a_pHandler->i32Integral > 0 && a_pHandler->i32Integral < a_pHandler->u16OutputLimitMin)
    {
    	a_pHandler->i32Integral = a_pHandler->u16OutputLimitMin;
    }
    else if(a_pHandler->i32Integral < 0 && a_pHandler->i32Integral > (int32_t)a_pHandler->u16OutputLimitMin*(-1))
    {
    	a_pHandler->i32Integral = (int32_t)a_pHandler->u16OutputLimitMin*(-1);
    }
    else if(a_pHandler->i32Integral < (int32_t)a_pHandler->u16OutputLimitMax*(-1))
    {
    	a_pHandler->i32Integral = (int32_t)a_pHandler->u16OutputLimitMax*(-1);
    }
    else
    {

    }
    i16RetVal = a_pHandler->i32Integral;

    return i16RetVal;

}



void Incremental_PID_Init (uint16_t Init_out, Incremental_PID_Object_t*  a_pHandler)
{
		a_pHandler->u16PID_Out = Init_out;
		a_pHandler->i16Error_last = 0;
		a_pHandler->i16dError_last = 0;
}


uint16_t Incremental_PID_Handler(Incremental_PID_Object_t * a_pHandler, int16_t error)
{
	int16_t d_error,dd_error;
	int32_t i32OutVal;

	a_pHandler->i16Error = error;
	d_error = a_pHandler->i16Error - a_pHandler->i16Error_last;
	dd_error = d_error - a_pHandler->i16dError_last;

	i32OutVal =  (int32_t)a_pHandler->u16PID_Out + ((int32_t)a_pHandler->u16KP * d_error + (int32_t)a_pHandler->u16KI * error + (int32_t)a_pHandler->u16KD * dd_error)/100;
	if(i32OutVal <= 0)
	{
		a_pHandler->u16PID_Out = 0;
	}
	else if(i32OutVal > a_pHandler->u16Out_Limit)
	{
		a_pHandler->u16PID_Out = a_pHandler->u16Out_Limit;
	}
	else
	{
		a_pHandler->u16PID_Out = i32OutVal;
	}

	a_pHandler->i16Error_last = a_pHandler->i16Error;
	a_pHandler->i16dError_last = d_error;

	return a_pHandler->u16PID_Out;
}



/***************************************Functions***********************************/
